/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Watchdog;
import scripting.CommandFactory;
import scripting.ScriptEngine;
import util.Vector2D;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTestingBoard extends IterativeRobot {

    private RobotParts robot;
    private ScriptEngine scriptEngine;
    private ImageTracker tracker;
    private Watchdog watchDog;
    private DataRecorder recorder;
    //gyro test vars (would not usually be in main class
    private Vector2D[] motorVectors = {
        new Vector2D(Math.cos(Math.toRadians(216)), Math.sin(Math.toRadians(216))),
        new Vector2D(Math.cos(Math.toRadians(324)), Math.sin(Math.toRadians(324))),
        new Vector2D(Math.cos(Math.toRadians(252)), Math.sin(Math.toRadians(252))),
        new Vector2D(-1, 0),
        new Vector2D(Math.cos(Math.toRadians(144)), Math.sin(Math.toRadians(144))),
        new Vector2D(Math.cos(Math.toRadians(108)), Math.sin(Math.toRadians(108))),
        new Vector2D(Math.cos(Math.toRadians(72)), Math.sin(Math.toRadians(72))),
        new Vector2D(Math.cos(Math.toRadians(36)), Math.sin(Math.toRadians(36))),
        new Vector2D(1, 0),
        new Vector2D(Math.cos(Math.toRadians(288)), Math.sin(Math.toRadians(288)))
    };
    private double max_speed = .3;
    private double scale_factor = max_speed / Math.sqrt(2);
    private boolean useGyro = false;
    //pid test vars
    private final int NONE = 0;
    private final int P = 1;
    private final int I = 2;
    private final int D = 3;
    private int selector = NONE;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        //get watchdog
        watchDog = Watchdog.getInstance();
        watchDog.feed();

        //recorder
        recorder = DataRecorder.getInstance();
        recorder.setPath("file:////ni-rt/startup/telemetry.csv");
        recorder.setWriteToFlashEnabled(true);

        String[] fields = {
            "Goal RPM",
            "Actual RPM",
            "PWM",
            "P",
            "I",
            "D"
        };
        recorder.createField(fields);

        //initialize robot parts
        robot = new RobotParts();

        //create ScriptEngine
        scriptEngine = new ScriptEngine(new CommandFactory(robot));

        //create image tracker
        tracker = new ImageTracker(robot.getCamera());

        //find the top target
        tracker.setTarget(ImageTracker.TOP_TARGET);

        //initialize ds
        DriverStation708.init(robot);

    }

    public void autonomousInit() {
        watchDog.feed();

        scriptEngine.abort();

        //make sure engine ready to parse
        scriptEngine.setReady();

        //schedule the autonomous script
        scriptEngine.initAutonomous();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        watchDog.feed();

        //run current script
        scriptEngine.run();

        //input
        dsInput(true);

        //output
        dsOutput();

    }

    public void disabledPeriodic() {
        //input
        dsInput(false);

        //output
        dsOutput();
    }

    public void teleopInit() {
        watchDog.feed();

        recorder.startTimer();

        //stop running autonomous commands
        scriptEngine.abort();

        //ready up
        scriptEngine.setReady();

        //start processing images
       tracker.start();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        watchDog.feed();

        //run manually scheduled scripts
        scriptEngine.run();

        //input
        dsInput(true);

        //output
        dsOutput();

    }

    public void disabledInit() {
        //write telemetry file
        recorder.writeToFile();

        //stop image processing
        tracker.stop();
    }

    private void dsInput(boolean enabled) {
        if (DriverStation708.runScript.getButtonState() && enabled) {
            scriptEngine.initAutonomous();
            //now wait for the script to finish
            scriptEngine.setWaiting();
        }

        if (DriverStation708.readNamesFile.getButtonState()) {
            //re-read file
            scriptEngine.readNamesFile();
        }

        if (DriverStation708.autoModeUp.getButtonState()) {
            //inc auto mode
            scriptEngine.nextAutoMode();
        }

        if (DriverStation708.autoModeDown.getButtonState()) {
            //dec auto mode
            scriptEngine.previousAutoMode();
        }

        if (DriverStation708.stopScripts.getButtonState()) {
            //stop scripts
            scriptEngine.abort();
            scriptEngine.setReady();

            //stop motors
            robot.stopMotors();
        }

        if (DriverStation708.resetGyro.getButtonState()) {
            //reset gyro
            robot.getGyro().reset();
        }

        if (DriverStation708.nextScreen.getButtonState()) {
            MessageCenter.getInstance().nextScreenNumber();
        } else if (DriverStation708.prevScreen.getButtonState()) {
            MessageCenter.getInstance().prevScreenNumber();
        }

        useGyro = DriverStation708.useGyro.getButtonState();

        //avoid clash with executing scripts
        if (!scriptEngine.hasScripts()) {
            gyroTest();
            pidTest();
        }
    }
    private int count = 0;

    private void dsOutput() {
        EncoderMotor motor = (EncoderMotor) robot.getMotor(RobotParts.CIM2);

        if (++count >= 10) {
            MessageCenter mc = MessageCenter.getInstance();
            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine1, "AMode: " + scriptEngine.getAutonomousModeName());
            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine2, "Gyro: " + ((useGyro) ? "" + robot.getGyro().getAngle() : "disabled"));
            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine3, "Pot: " + robot.getPotentiometer().getAverageValue());

            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine4, "Goal RPM: " + motor.getGoalRPM());
            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine5, "Current RPM: " + motor.getRPM());
            mc.println(MessageCenter.kFirstScreen, MessageCenter.kLine6, "Current PWM: " + motor.get());

            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine1, "P: " + MessageCenter.formatDouble(motor.getP(), 6));
            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine2, "I: " + MessageCenter.formatDouble(motor.getI(), 6));
            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine3, "D: " + MessageCenter.formatDouble(motor.getD(), 6));

            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine4, "Offset px: " + tracker.getMostRecentMovement());
            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine5, "EstDist in: " + tracker.getMostRecentDistance());
            mc.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine6, "NumParts: " + tracker.getMostRecentParticleCount());

            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine1, "M1010: " + robot.getUltrasonic1().getDistance());
            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine2, "M1340: " + robot.getUltrasonic2().getDistance());
            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine3, "IR1: " + robot.getIR1().getDistance());
            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine4, "IR2: " + robot.getIR2().getDistance());
            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine5, "IR1V: " + robot.getIR1().getVoltage());
            mc.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine6, "IR2V: " + robot.getIR2().getVoltage());

            mc.println(MessageCenter.kEncodersScreen,MessageCenter.kLine1, "RPM: " + robot.getTestEncoder().getRPM());
            mc.println(MessageCenter.kEncodersScreen,MessageCenter.kLine2, "DistSec: " + robot.getTestEncoder().getRate());
            mc.println(MessageCenter.kEncodersScreen,MessageCenter.kLine3, "Count: " + robot.getTestEncoder().get());

            mc.display();

            count = 0;
        }

        //log data
        recorder.record("Goal RPM", motor.getGoalRPM());
        recorder.record("Actual RPM", motor.getRPM());
        recorder.record("PWM", motor.get());

        recorder.record("P", motor.getP());
        recorder.record("I", motor.getI());
        recorder.record("D", motor.getD());

        recorder.log();
        

    }

    //private int count2 = 0;

    /*
     * This method tests a PID controller.
     */
    private void pidTest() {
//        if(++count2 > 300){
//            EncoderMotor m = (EncoderMotor) robot.getMotor(RobotParts.CIM2);
//            m.set(robot.getPotentiometer().getValue() / 1000.0);
//
//            count2 = 0;
//        }
    }

    /**
     * This method carries out a test of the
     * gyroscope which uses the gamepad joystick input.
     * (and looks really cool!)
     */
    private void gyroTest() {
        Vector2D motorSpeed = Vector2D.scratch;
        Vector2D gamepad = Vector2D.scratch2;
        double d, cosTheta, sinTheta, gyroAngle;

        //get the vector of the joystick
        gamepad.setTo(DriverStation708.gamepad.getAxis(Gamepad.rightStick_X), DriverStation708.gamepad.getAxis(Gamepad.rightStick_Y));
        gamepad.y *= -1;

        if (useGyro) {
            //get the angle indicated by the gyro
            gyroAngle = robot.getGyro().getAngle();
            cosTheta = Math.cos(Math.toRadians(gyroAngle));
            sinTheta = Math.sin(Math.toRadians(gyroAngle));

            //rotate joystick angle by gyro angle
            gamepad.x = gamepad.x * cosTheta - gamepad.y * sinTheta;
            gamepad.y = gamepad.x * sinTheta + gamepad.y * cosTheta;
        }

        //set each motor to the scaled dot product of the gamepad and that motor's normalized vector.
        for (int i = 0; i < robot.getTotalMotors(); i++) {
            motorSpeed.setTo(motorVectors[i]);
            d = gamepad.getDotProduct(motorSpeed) * scale_factor;

            if (d >= 0) {
                setMotor(i, d);
            }
        }
    }

    private void setMotor(int index, double speed) {
        speed = Math.min(speed, .3);
        speed = Math.max(0, speed);

        Object o = robot.getMotor(index);
        if (o != null) {
            if (o instanceof SpeedController) {
                SpeedController s = (SpeedController) o;
                s.set(speed);
            } else if (o instanceof Relay) {
//                    Relay r = (Relay) o;
//
//                    if (speed > 0) {
//                        r.set(Relay.Value.kForward);
//                    } else if (speed < 0) {
//                        r.set(Relay.Value.kReverse);
//                    } else {
//                        r.set(Relay.Value.kOff);
//                    }
            }
        } else {
            System.out.println("Non-existent motor accessed.");
        }
    }
}
